/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "gripperinstructionlogic.h"
#include "GeneralDefine.h"
#include "programengine.h"
#include "powermanager.h"

extern PowerManager *p_powerManger;

GripperInstructionLogic::GripperInstructionLogic(int robotIdIn,ProgramSet* programSetIn,
                                                 RobotIo* ioControllerIn,ProgramEngine* programEngineIn)
{
  programSet=programSetIn;
  robotId=robotIdIn;
  ioController=ioControllerIn;
  programEngine=programEngineIn;
}

E_PROGRAM_LOOP_RESULT GripperInstructionLogic::startGripper(int programNum,
                                                            GripperInstruction gripperInstruct)
{
    programEngine->updateProgramSetRegister();
    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

    if(ENUM_GRIPPER_ACTIVE == gripperInstruct.gripperType)
    {
        //调用ActiveGripper
        bool activeGripperFlag = false;
        //activeGripperFlag = ActiveGripper();
         if( p_powerManger ){
            activeGripperFlag = p_powerManger->connectRobotiq();
            qDebug()<< "activeGripperFlag==" << activeGripperFlag;
         }
        if(activeGripperFlag)
        {
            programEngine->calculateNextLineToRun(programNum);
        }
        else
        {
            //error
            int jumpNum;
            if(INDIRECT == gripperInstruct.type)
            {
                if((D_ROBOT_REG_SIZE < gripperInstruct.i)||(1 > gripperInstruct.i))
                {
                    programEngine->cmdError(programNum);
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                else
                {
                    jumpNum = programSet->regRList[gripperInstruct.i - 1];
                }
            }
            else
            {
                jumpNum = gripperInstruct.i;
            }
            QString tmpjumpLabel=QString::number(jumpNum);
            programEngine->startJump(programNum,tmpjumpLabel);
        }
    }
    else if(ENUM_GRIPPER_SET == gripperInstruct.gripperType)
    {
        //调用SetGripper
     //   jumpNum = programSet->r[gripperInstruct.i - 1];
        if( p_powerManger ){
            p_powerManger->robotiq_clamp_move( gripperInstruct.gripperPosition ,gripperInstruct.gripperSpeed, gripperInstruct.gripperForce);
            qDebug()<< "调用SetGripper==";
        }
        programEngine->calculateNextLineToRun(programNum);
    }
    else if(ENUM_GRIPPER_WAIT == gripperInstruct.gripperType)
    {
        //调用WaitGripper
        bool gripperFinished = false;
        bool completeFlag = false;
        //gripperFinished = WaitGripper();
        if(gripperFinished)
        {
            programEngine->calculateNextLineToRun(programNum);
        }
        else
        {
            completeFlag = programEngine->countWaitTime(gripperInstruct.waitTime);
            if(completeFlag)
            {
                int jumpNum;
                if(INDIRECT == gripperInstruct.type)
                {
                    if((D_ROBOT_REG_SIZE < gripperInstruct.i)||(1 > gripperInstruct.i))
                    {
                        programEngine->cmdError(programNum);
                        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        jumpNum = programSet->regRList[gripperInstruct.i - 1];
                    }
                }
                else
                {
                    jumpNum = gripperInstruct.i;
                }
                QString tmpjumpLabel=QString::number(jumpNum);
                programEngine->startJump(programNum,tmpjumpLabel);
            }
        }
    }
}


void GripperInstructionLogic::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 3017:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",寄存器指令错误，寄存器的范围超过限定的最大范围，请修改");
    }
        break;



    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}
